#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "tfs_sub");
    ros::NodeHandle nh;
    // 创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    ros::Rate rate(10.0);
    // 创建坐标点
    geometry_msgs::PointStamped ps_son1;
    ps_son1.header.stamp = ros::Time::now();
    ps_son1.header.frame_id = "son1";
    ps_son1.point.x = 1.0;
    ps_son1.point.y = 2.0;
    ps_son1.point.z = 3.0;


    while (ros::ok())
    {
        try
        {
            //1. 计算son1和son2的相对关系
            /*
            参数1：目标坐标系
            参数2：源坐标系
            参数3： ros::Time(0), 取间隔最短的两个坐标关系帧计算相对关系
            返回值： geometry_msgs::TransformStamped son12son2 源相对于目标坐标系的一个相对关系
            */
            geometry_msgs::TransformStamped son12son2 = buffer.lookupTransform("son2", "son1", ros::Time(0));
            ROS_INFO("son1相对于son2的关系： 父级 : %s, 子级 : %s, 偏移量 : (%.2f, %.2f, %.2f)", 
                                        son12son2.header.frame_id.c_str(), son12son2.child_frame_id.c_str(),
                                        son12son2.transform.translation.x,son12son2.transform.translation.y, son12son2.transform.translation.z);
            //2. 计算son1的某个坐标点在son2中的坐标值
            geometry_msgs::PointStamped ps_out2 =  buffer.transform(ps_son1, "son2");
            ROS_INFO("坐标点在son2中的值 （%.2f, %.2f, %.2f）", ps_out2.point.x,  ps_out2.point.y,  ps_out2.point.z);
        }
        catch(const std::exception& e)
        {
            std::cerr << e.what() << '\n';
        }
        
    }
    
}